#include "U_cv.h"

// using namespace std;
// using namespace cv;

int s1_ray_to_xy(Mat &depth, Mat camera_intrsic)
{
	float cx = camera_intrsic.at<float>(0, 0);
	float cy = camera_intrsic.at<float>(1, 0);
	float fx = camera_intrsic.at<float>(2, 0);
	float fy = camera_intrsic.at<float>(3, 0);

	float *lens_ray_to_xy = (float *)calloc(depth.rows* depth.cols, sizeof(float));
	float atan_value = 0;
	for (int i = 0; i < depth.rows; i++)
		for (int j = 0; j < depth.cols; j++)
		{
			atan_value = sqrt((pow((j - cx) / fx, 2) + pow((i - cy) / fy, 2)));
			lens_ray_to_xy[i* depth.cols + j] = 1 / (sqrt(1 + atan_value*atan_value));
		}

	for (int i = 0; i < depth.rows; i++)
		for (int j = 0; j< depth.cols; j++)
		{
			if (depth.at<float>(i, j) != 0)
			{
				depth.at<float>(i, j) = depth.at<float>(i, j) * lens_ray_to_xy[i*depth.cols + j];// *1 / (sqrt(1 + atan_value*atan_value));
			}
		}
	free(lens_ray_to_xy);
	return 0;
}

void version_print(void)
{
	std::cout << "OpenCV version: " << CV_VERSION << std::endl;
}



int lens_new(void)
{

	std::ifstream fin("calibdata.txt");					//标定所用图像文件路径
	ofstream fout("caliberation_result.txt");			//保存标定结果的文件
	cout << "开始提取角点..." << endl;
	int image_count = 0;								//图像数量
	Size image_size;									//图像尺寸
	Size board_size = Size(4,6);						//标定板上每行列的角点数
	vector<Point2f> image_points_buf;					//缓存每幅图像上检测的角点
	vector<vector<Point2f>> image_points_seq;			//保存检测到的所有角点
	string filename;

	int count = -1;
	while (getline(fin,filename))						//获取文本行
	{
		cout<< filename << endl;						//打印
		image_count++;
		cout<<"image_count = "<<image_count<<endl;
		// cout<<"-->count = "<<count;
		Mat imageInput = imread(filename);				//读取文件路径到imageInput
		if (image_count == 1)
		{
			image_size.width = imageInput.cols;
			image_size.height = imageInput.rows;
			cout<<"image_size.width = " << image_size.width<<endl;
			cout<<"image_size.height = " << image_size.height<<endl;
		}
		/* 提取角点 */
		//							  (文件读入,	标定板行列数，	缓存图片检测角点)
		if (0 == findChessboardCorners(imageInput,board_size,image_points_buf))
		{
			cout << "can not find chessboard corner!\n";
			exit(1);
		} else
		{
			Mat view_gray;
			//		(输入图像，	  输出图像，	rgb 转 灰度)
			// cvtColor(imageInput,view_gray,CV_RGB2GRAY);
			cvtColor(imageInput,view_gray,COLOR_BGR2GRAY);
			//				 	 (输入mat矩阵，初始角点坐标     ，角点搜索窗口的尺寸)
			find4QuadCornerSubpix(view_gray,image_points_buf,Size(5,5));
			//上传
			image_points_seq.push_back(image_points_buf);
			//绘制被成功标定的角点-->(8位灰度，标定板行列数，初始的角点坐标向量，)
			drawChessboardCorners(view_gray,board_size,image_points_buf,false);
			imshow("Camera Calibration",view_gray);
			cout<<"now show = "<< image_count << endl;
			waitKey(500);
		}
	}

	int total = image_points_seq.size();
	cout<<"tatal = "<< total <<endl;
	int CornerNum = board_size.width * board_size.height;
	for (int ii = 0; ii < total; ii++)
	{
		if (0 == ii % CornerNum)
		{
			int i = -1;
			i = ii / CornerNum;
			int j = i + 1;
			cout<<"--> 第 " << j <<" 图像数据 --> :"<<endl;
		}
		if (0 == ii % 3)
		{
			cout<<endl;
		}
		else
		{
			cout.width(10);
		}
		cout<<" -->"<<image_points_seq[ii][0].x;
		cout<<" -->"<<image_points_seq[ii][0].y;	
	}
	cout << "角点提取完成!\n";
	cout << "开始标定..";
	Size square_size = Size(10,10);//棋盘格实际大小
	vector<vector<Point3f>> object_points;//保存标定板上角点三维坐标
	Mat cameraMatrix = Mat(3,3,CV_32FC1,Scalar::all(0));//摄像机内参数矩阵
	vector<int> point_counts;//每幅图像中角点数量
	Mat distCoeffs = Mat(1,5,CV_32FC1,Scalar::all(0));//摄像机的5个畸变参数
	vector<Mat> tvecsMat;//每幅图旋转向量
	vector<Mat> rvecsMat;//每幅图平移向量
	int i,j,t;
	for (t = 0; t < image_count; t++)
	{
		vector<Point3f> tempPointSet;
		for (i = 0; i < board_size.height; i++)
		{
			for (j = 0; j < board_size.width; j++)
			{
				Point3f realPoint;
				realPoint.x = i * square_size.width;
				realPoint.y = j * square_size.height;
				realPoint.z = 0;
				tempPointSet.push_back(realPoint);
			}
		}
		object_points.push_back(tempPointSet);
	}
	for (i = 0; i < image_count; i++)
	{
		point_counts.push_back(board_size.width * board_size.height);
	}
	//			   (世界坐标系三维点，内角点对应的图像坐标点,图像的像素尺寸大小,相机内参，畸变矩阵,旋转向量,位移向量,标定时所采用的算法)
	calibrateCamera(object_points,image_points_seq,image_size,cameraMatrix,distCoeffs,rvecsMat,tvecsMat,0);
	cout<<"标定完成\n";
	cout<<"开始评价标定结果....\n";
	double total_err = 0.0;			//所有图像平均误差的总和
	double err = 0.0;				//每幅图像的平均值
	vector<Point2f> image_points2;	//保存重新计算得到的投影点
	cout << "\t每幅图像的标定误差:\n";
	fout << "每幅图像的标定误差:\n";
	for (i = 0; i < image_count; i++)
	{
		vector<Point3f> tempPointSet = object_points[i];
		//通过得到相机内外参数对空间三维点进行重新投影计算得到新的投影点
		//			 (相机坐标系中的三维点坐标,旋转向量,位移向量,相机内参,		畸变矩阵,	重新计算得到的投影点)
		projectPoints(tempPointSet,rvecsMat[i],tvecsMat[i],cameraMatrix,distCoeffs,image_points2);
		//计算新的投影点与旧的投影点之间的误差
		vector<Point2f> tempImagePoint = image_points_seq[i];
		Mat tempImagePointMat = Mat(1,tempImagePoint.size(),CV_32FC2);
		Mat image_points2Mat = Mat(1,image_points2.size(),CV_32FC2);
		for (int j = 0; j < tempImagePoint.size(); j++)
		{
			image_points2Mat.at<Vec2f>(0,j) = Vec2f(image_points2[j].x,image_points2[j].y);
			tempImagePointMat.at<Vec2f>(0,j) = Vec2f(tempImagePoint[j].x,tempImagePoint[j].y);
		}
		err = norm(image_points2Mat,tempImagePointMat,NORM_L2);
		total_err += err /= point_counts[i];
		cout<<"第"<<i+1<<"幅图像的平均误差:"<<err<<"像素"<<endl;
		fout<<"第"<<i+1<<"幅图像的平均误差:"<<err<<"像素"<<endl;
	}
	cout << "总体平均误差:"<<total_err / image_count << "像素" << endl;
	fout << "总体平均误差:"<<total_err / image_count << "像素" << endl;
	cout << "评价完成！" << endl;
	cout << "开始保存标定结果..." << endl;
	Mat rotation_matrix = Mat(3,3,CV_32FC1,Scalar::all(0));
	fout << "相机内参矩阵:" << endl;
	fout << cameraMatrix << endl;
	fout << "畸变系数:" << endl;
	fout << distCoeffs <<endl;
	for (int i = 0; i < image_count; i++)
	{
		fout << "第" << i + 1 << "幅图像的旋转向量:" << endl;
		fout << tvecsMat[i] << endl;
		//旋转向量转为旋转矩阵
		Rodrigues(tvecsMat[i],rotation_matrix);
		fout << "第" << i + 1 << "幅图像的旋转矩阵:" << endl;
		fout << rotation_matrix << endl;
		fout << "第" << i + 1 << "幅图像的平移向量:" << endl;
		fout << rvecsMat[i] << endl; 
	}
	cout << "完成保存" << endl;
	fout << endl;
	// 显示结果
	Mat mapx = Mat(image_size, CV_32FC1);
	Mat mapy = Mat(image_size, CV_32FC1);
	Mat R = Mat::eye(3,3,CV_32F);
	cout << "保存矫正图像" << endl;
	string imageFileName;
	stringstream StrStm;
	for (int i = 0; i != image_count; i++)
	{
		cout << "Frame #" << i+1 << "..." << endl;
		initUndistortRectifyMap(cameraMatrix,distCoeffs,R,cameraMatrix,image_size,CV_32FC1,mapx,mapy);
		StrStm.clear();
		imageFileName.clear();
		// /home/liu/Desktop/User_y/code/linux/code_study/U_cpp_demo/test_0/source/correct
		string User_filePath = "/home/liu/Desktop/User_y/code/linux/code_study/U_cpp_demo/test_0/source/correct/";
		string filePath = "chess";
		StrStm << i + 1;
		StrStm >> imageFileName;
		filePath += imageFileName;
		filePath += ".png";
		string all_path = User_filePath + filePath;
		// cout << filePath << endl;
		cout << all_path << endl;

		//frame2.png
#if 0
		// int width = 640;  // 图像宽度			image_size.width
    	// int height = 480; // 图像高度			image_size.height
		int width = image_size.width;  // 图像宽度			image_size.width
    	int height = image_size.height; // 图像高度			image_size.height
		cv::Mat whiteImage = cv::Mat::ones(height, width, CV_8UC3) * 255 ;
		// std::string filename = "chess1.bmp";//filePath
		std::string filename = filePath;//filePath
		imwrite(filename, whiteImage);
#endif
// /home/liu/Desktop/User_y/code/linux/code_study/U_cpp_demo/test_0/source/frame3.png

		// Mat imageSource = imread(filePath);	//all_path
		Mat imageSource = imread(all_path);	//all_path

		// Mat imageSource = imwrite(filePath);
		Mat newimage = imageSource.clone();

		remap(imageSource,newimage,mapx,mapy,INTER_LINEAR);
		StrStm.clear();
		filePath.clear();
		StrStm << i + 1;
		StrStm >> imageFileName;
		imageFileName += "_d.jpg";
		imwrite(imageFileName,newimage);
 		
	}
	cout<<"保存结束"<<endl;
	return 0;
}





























